/**************************************************************
 * Ben Stephenson 07025656
 * CM0034 Industrial Case Project
 * Control Panel
 * main.c
 * 28/02/2011
 **************************************************************/
/**************************************
 ************ PRE-PROCESSOR************
 *^z***********************************/
#include <stdarg.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <ucos_ii.h>
#include <board.h>
#include <bsp.h>
#include <osutils.h>
#include <leds.h>
#include <buttons.h>
#include <lcd.h>
#include <interface.h>
#include <control.h>

#include <messages.h>
#include <state.h>

#define  APP_TASK_EMERG_STOP_PRIO   4
#define  APP_TASK_CAN_READ_PRIO     5
#define  APP_TASK_IN_SENSOR_PRIO    6
#define  APP_TASK_BUTTONS_PRIO      7
#define  APP_TASK_LEDS_PRIO         8
#define  APP_TASK_LCD_PRIO          9
#define  APP_TASK_LIST_END         -1

#define  TASK_STK_SIZE              256
/*z^***********************************/


/**************************************
 ************* PROTOTYPES *************
 *^z***********************************/
static void appTaskButtons(void *pdata);
static void appTaskLeds(void *pdata);
static void appTaskCanRead(void *pdata);
static void appTaskInSensor(void *pdata);
static void appTaskLCD(void *pdata);
static void appTaskEmergStop(void *pdata);

static void initTasks(void);
static void killTasks(int, ...);
static void initiateEmergStop(void);
static void checkPadSensorOne(messageData_t*);
static void checkPadSensorTwo(messageData_t*);
/*z^***********************************/


/**************************************
 ********** GLOBAL VARIABLES **********
 *^z***********************************/

/*z^***********************************/


/**************************************
 ************* TASK STACKS ************
 *^z***********************************/
static OS_STK appTaskButtonsStk[TASK_STK_SIZE];
static OS_STK appTaskLedsStk[TASK_STK_SIZE];
static OS_STK appTaskCanReadStk[TASK_STK_SIZE];
static OS_STK appTaskSensorsStk[TASK_STK_SIZE];
static OS_STK appTaskLCDStk[TASK_STK_SIZE];
static OS_STK appTaskEmergStopStk[TASK_STK_SIZE];
/*z^***********************************/

//--------------------------------------------------------------------------------------------------------------------------//

/*************************************************
 * Function : main
 * Desc     : Main Function.
 * Return   : int
 * Params   : void *
 *^z**********************************************/
int main()
{
   /* Initialise the OS */
   OSInit();                                                   

   /* Create Tasks */
   initTasks();
  
   /* Start the OS */
   OSStart();                                                  
  
   /* Should never arrive here */ 
   return 0;      
}
/*z^**********************************************/


/********************************************************************************************
 *                                  APPLICATION TASK DEFINITIONS
 *^z*****************************************************************************************/

/*************************************************
 * Function : appTaskButtons
 * Desc     : Reads the buttons states
 * Return   : Void
 * Params   : void *
 *^z**********************************************/
static void appTaskButtons(void *pdata)
{
   //Initialise the hardware and start the OS ticker 
   //Note this must be done in the highest priority task
   bspInit();
   controlInit();
   osStartTick();
   initState();
   initBuff();

   messageData_t msg;

   while (true)
   {
      // Pause only active if we are running 
      //spec2.4  spec3.1.2
      if (isButtonPressed(BUT_1) &&
          (RUNNING == getState() ||
           PAUSED  == getState()))
      {
         setNodeState(MY_ID, PAUSED);
         msg.targetID     = ID_ALLNODES;
         msg.originatorID = MY_ID;
         msg.content      = MSG_PAUSE;
         writeMsgToCan(&msg);  
      }
      // If the stop button is pressed, purge cell and stop
      // only active if we are running
      //spec2.2  spec3.1.1
      else if (isButtonPressed(JS_DOWN) &&
               RUNNING == getState())
      {
         setNodeState(MY_ID, NODE_STOPPED);
         msg.targetID     = ID_ARM1;
         msg.originatorID = MY_ID;
         msg.content      = MSG_STOP;
         writeMsgToCan(&msg);

         setState(STOPPING);
      }
      else if (isButtonPressed(JS_UP))
      {
         setNodeState(MY_ID, NODE_READY);
         msg.targetID     = ID_ALLNODES;
         msg.originatorID = MY_ID;
         msg.content      = MSG_RESUME;
         writeMsgToCan(&msg);  
      }
      // The reset button should only have an effect if we are in an
      // error state already - spec3.1.3

      // We only want to be able to initite an emergency stop
      // if we are not already in an emergency state
      // spec2.5  spec3.1.4
      else if (controlEmergencyStopButtonPressed() &&
               EMERG != getState())
      {
         setNodeState(MY_ID,
                      NODE_EMERG);

         msg.targetID     = ID_ALLNODES;
         msg.originatorID = MY_ID;
         msg.content      = MSG_EMERG_STOP;
         writeMsgToCan(&msg);
         initiateEmergStop();
      }     

      OSTimeDly(50);
   }
}
/*z^**********************************************/


/*************************************************
 * Function : appTaskLCD
 * Desc     : Updates the LCD
 * Return   : Void
 * Params   : void *
 *^z**********************************************/
static void appTaskLCD(void *pdata)
{
   lcdWrite("\f");
   messageData_t last_msg[5];

   while(true)
   {
      getLastMsg(&last_msg[MY_ID], MY_ID);
      getLastMsg(&last_msg[ID_ARM1], ID_ARM1);
      getLastMsg(&last_msg[ID_ARM2], ID_ARM2);
      getLastMsg(&last_msg[ID_CONVEYOR], ID_CONVEYOR);

      lcdSetTextPos(2,1);
      switch (getState())
      {
         case INIT:
            lcdWrite("Initialising ");
         break;
         case EMERG:
            lcdWrite("Emerg Stop   ");
         break;
         case READY:
            lcdWrite("Cell Ready   ");
         break;
         case RUNNING:
            lcdWrite("Cell Running ");
         break;
         case PAUSED:
            lcdWrite("Cell Paused  ");
         break;
         case STOPPING:
            lcdWrite("Cell Stopping");
         break;
         case STOPPED:
            lcdWrite("Cell Stopped ");
         break;
         default: break;
      }
      
      lcdSetTextPos(2, 2);
      lcdWrite("  W: %d: %d: %d: %d ", last_msg[MY_ID].originatorID,
                                       last_msg[MY_ID].targetID,
                                       last_msg[MY_ID].content,
                                       getNodeState(MY_ID));
      lcdSetTextPos(2,3);
      lcdWrite("A1R: %d: %d: %d: %d ", last_msg[ID_ARM1].originatorID,
                                       last_msg[ID_ARM1].targetID,
                                       last_msg[ID_ARM1].content,
                                       getNodeState(ID_ARM1));
      lcdSetTextPos(2,4);
      lcdWrite("CVR: %d: %d: %d: %d ", last_msg[ID_CONVEYOR].originatorID,
                                       last_msg[ID_CONVEYOR].targetID,
                                       last_msg[ID_CONVEYOR].content,
                                       getNodeState(ID_CONVEYOR));
      lcdSetTextPos(2,5);
      lcdWrite("A2R: %d: %d: %d: %d ", last_msg[ID_ARM2].originatorID,
                                       last_msg[ID_ARM2].targetID,
                                       last_msg[ID_ARM2].content,
                                       getNodeState(ID_ARM2));
      lcdSetTextPos(1, 8);
      lcdWrite("                     ");

      if (NODE_EMERG == getNodeState(MY_ID))
      {
         lcdSetTextPos(4, 7);
         lcdWrite("EMERGENCY STOP");
         lcdSetTextPos(2, 9);
         lcdWrite("RESET : HOLD BUT2");
      }


      if (EMERG   == getState() ||
          STOPPED == getState())
      {
         lcdSetTextPos(1, 8);
         lcdWrite("ALL NODES REPORT STOP");
      }
      OSTimeDly(300);
   }
}
/*z^**********************************************/


/*************************************************
 * Function : appTaskLeds
 * Desc     : Handles the status LEDs
 * Return   : Void
 * Params   : void *
 *^z**********************************************/
static void appTaskLeds(void *pdata)
{
   while (true)
   {
      // Running LED is on when the whole system is running
      // flashing when waiting to be running or waiting to
      // be stopped, otherwise off
      
      // spec3.1.1
      if (RUNNING == getState())
      {
         interfaceLedSetState(D4_LED, LED_ON);
      }
      else if (NODE_RUNNING == getNodeState(MY_ID)  ||
               STOPPING     == getState())
      {
         interfaceLedToggle(D4_LED);
      }
      else
      {
         interfaceLedSetState(D4_LED, LED_OFF);
      }

      // Paused LED is on when the whole system is running
      // flashing when waiting for everyone to pause, otherwise off
      // spec3.1.2
      if (PAUSED == getState())
      {
         interfaceLedSetState(D3_LED, LED_ON);
      }
      else if (NODE_PAUSED == getNodeState(MY_ID))
      {
            interfaceLedToggle(D3_LED);
      }
      else
      {
         interfaceLedSetState(D3_LED, LED_OFF);
      }

      // Ready LED is on when the whole system is running
      // flashing when waiting to be ready, otherwise off
      // spec3.1.3
      if (READY == getState())
      {
         interfaceLedSetState(D2_LED, LED_ON);
         // If everyone is ready, we are running!
         setNodeState(MY_ID, NODE_RUNNING);
         setNodeState(ID_ARM1, NODE_RUNNING);
         setNodeState(ID_ARM2, NODE_RUNNING);
         setNodeState(ID_CONVEYOR, NODE_RUNNING);
         setState(RUNNING);
      }
      else if (NODE_READY == getNodeState(MY_ID))
      {
         interfaceLedToggle(D2_LED);
      }
      else
      {
         interfaceLedSetState(D2_LED, LED_OFF);
      }

      OSTimeDly(200);
   }
}
/*z^**********************************************/


/*************************************************
 * Function : appTaskSensors
 * Desc     : Checks the sensors for a block
 * Return   : Void
 * Params   : void *
 *^z**********************************************/
static void appTaskInSensor(void *pdata)
{
   messageData_t  msg,
                  last_msg;

   while (true)
   {
      // If the system isn't ready, we don't want to kick off 
      // an action.
      while (RUNNING == getState())
      {
         ledSetState(USB_LINK_LED, LED_OFF);
         // Check if there is anything on the IN PAD
         // so we can tell ARM1 to pick up the block.
         // spec3.1.6
         if (controlItemPresent(CONTROL_SENSOR_1))
         {    
            // If ARM1 already knows then the last_msg will be an
            // ACK, if it is, don't send another message.
            getLastMsg(&last_msg,
                       ID_ARM1);
   
            if (MSG_ACKNOLEDGE != last_msg.content)
            {
               ledSetState(USB_LINK_LED, LED_ON);
               // Tell the input arm there's something to pickup
               msg.targetID     = ID_ARM1;
               msg.originatorID = MY_ID;
               msg.content      = MSG_SENS1_FULL;
               writeMsgToCan(&msg);
            }
         }
         // We only want to check the OUT PAD when ARM2
         // asks us if it has dropped off a block.
         OSTimeDly(20);
      }

      OSTimeDly(40);
   }
}

/*z^**********************************************/


/*************************************************
 * Function : appTaskCanRead
 * Desc     : Reads messages from the bus
 * Return   : Void
 * Params   : void *
 *^z**********************************************/
static void appTaskCanRead(void *pdata)
{
   messageData_t read_msg;
   messageData_t write_msg;

   while (true)
   {
      memset(&read_msg, 0, sizeof(read_msg));
      readMsgFromCan(&read_msg);

      ledToggle(USB_LINK_LED);

      switch(read_msg.content)
      {
         case MSG_PAD_SENS1_STAT_REQ:
         {
            checkPadSensorOne(&write_msg);
            writeMsgToCan(&write_msg);
         }
         break;

         // Tell ARM2 if there is something on the OUT pad
         // spec3.1.6
         case MSG_PAD_SENS2_STAT_REQ:
         {
            checkPadSensorTwo(&write_msg);
            writeMsgToCan(&write_msg);
         }
         break;

         case  MSG_PICKUP_DEPOSIT_DONE:
         {
            // Checkif the ARMs have picked up/dropped off the block
            // sucessfully or not by checking the appropriate sensor
            if (ID_ARM1 == read_msg.originatorID)
            {
               checkPadSensorOne(&write_msg);
            }
            else if (ID_ARM2 == read_msg.originatorID)
            {
               checkPadSensorTwo(&write_msg);
            }
            writeMsgToCan(&write_msg);
         }
         break;

         case MSG_PAUSE:
         {
            // If we have issued a pause command, we will already be paused
            if (getNodeState(MY_ID) == NODE_PAUSED)
            {
               setNodeState(read_msg.originatorID,
                            NODE_PAUSED);
               determineState(PAUSED);
            }
         }
         break;

         // Wait for nodes to confirm they are ready
         case MSG_READY:
         {
            if (NODE_READY == getNodeState(MY_ID))
            {
               setNodeState(read_msg.originatorID,
                            NODE_READY);
               determineState(READY);
            }
         }
         break;

         // Wait for nodes to confirm they have resumed
         case MSG_RESUME:
         {
            if (NODE_READY == getNodeState(MY_ID))
            {
               setNodeState(read_msg.originatorID,
                            NODE_READY);
               determineState(READY);
            }
         }
         break;
         // Wait for each ndoe to confirm they have finished their current
         // action and have stopped
         case MSG_STOP:
         {
            setNodeState(read_msg.originatorID,
                         NODE_STOPPED);

            if (read_msg.originatorID != ID_ARM2)
            {
               write_msg.targetID     = read_msg.originatorID + 1;
               write_msg.originatorID = MY_ID;
               write_msg.content      = MSG_STOP;
               writeMsgToCan(&write_msg);
            }
            else
               setState(STOPPED);
         }
         break;

         // Wait for each node to halt immediatley
         case MSG_EMERG_STOP:
         {
            if (NODE_EMERG != getNodeState(MY_ID))
            {
               setNodeState(MY_ID,
                            NODE_EMERG);

               write_msg.targetID     = ID_ALLNODES;
               write_msg.originatorID = MY_ID;
               write_msg.content      = MSG_EMERG_STOP;
               writeMsgToCan(&write_msg);
   
               initiateEmergStop();
            }

            setNodeState(read_msg.originatorID,
                         NODE_EMERG);

            determineState(EMERG);
         }

         default: // Do nothing, should only get here if we have polled
                  // but not actually received a message.
         break;
      }
      OSTimeDly(10);
   }
}
/*z^**********************************************/


/*************************************************
 * Function : appTaskEmergStop
 * Desc     : Halt the system
 * Return   : 
 * Params   :
 *^z**********************************************/
void appTaskEmergStop(void *pdata)
{

   // Prevent the other tasks from running
   killTasks(APP_TASK_BUTTONS_PRIO,
             APP_TASK_LEDS_PRIO,
             APP_TASK_IN_SENSOR_PRIO,
             APP_TASK_LIST_END);

   controlAlarmSetState(CONTROL_ALARM_ON);
   ledSetState(USB_LINK_LED, LED_ON);
   ledSetState(USB_CONNECT_LED, LED_OFF);

   while (true)
   {
      ledToggle(USB_LINK_LED);
      ledToggle(USB_CONNECT_LED);
      controlAlarmToggleState();

      if (isButtonPressed(BUT_2))
      {
         // Don't need the LCD or CAN tasks anymore
         killTasks(APP_TASK_LCD_PRIO,
                   APP_TASK_CAN_READ_PRIO,
                   APP_TASK_LIST_END);

         // Re-init the core tasks
         initTasks();

         // Kill self
         killTasks(OS_PRIO_SELF
                   APP_TASK_LIST_END);
         
         break;
      }
      OSTimeDly(500);
   }
}
/*z^**********************************************/
/*z^*****************************************************************************************/

/********************************************************************************************
 *                                  LOCAL FUNCTION DEFINITIONS
 *^z*****************************************************************************************/
/*************************************************
 * Function : initTasks
 * Desc     : Initialises tasks for normal operation
 * Return   : void
 * Params   : void
 *^z**********************************************/
static void initTasks()
{
   OSTaskCreate(appTaskButtons,                               
                (void *)0,
                (OS_STK *)&appTaskButtonsStk[TASK_STK_SIZE - 1],
                APP_TASK_BUTTONS_PRIO);
   
   OSTaskCreate(appTaskLeds,                               
                (void *)0,
                (OS_STK *)&appTaskLedsStk[TASK_STK_SIZE - 1],
                APP_TASK_LEDS_PRIO);

   OSTaskCreate(appTaskCanRead,                               
                (void *)0,
                (OS_STK *)&appTaskCanReadStk[TASK_STK_SIZE - 1],
                APP_TASK_CAN_READ_PRIO);

   OSTaskCreate(appTaskInSensor,
                (void *)0,
                (OS_STK *)&appTaskSensorsStk[TASK_STK_SIZE - 1],
                APP_TASK_IN_SENSOR_PRIO);

   OSTaskCreate(appTaskLCD,
                (void *)0,
                (OS_STK *)&appTaskLCDStk[TASK_STK_SIZE - 1],
                APP_TASK_LCD_PRIO);
}
/*z^**********************************************/


/*************************************************
 * Function : killTasks
 * Desc     : Kills tasks listed in args list. The
 *            list is termianted with a trailing -1
 * Return   : void
 * Params   : int, ...
 *^z**********************************************/
static void killTasks
(
   int task_prio,
   ...
)
{
   va_list arg_list;
 
   // Get the first argument
   va_start(arg_list,
            task_prio); 

   // Cycle through the arg list and process each item
   for (int i = task_prio; i >= 0; i = va_arg(arg_list, int))
   {
      OSTaskDel(i);
   }

   // Tidy up after ourselves.
   va_end(arg_list);
}
/*z^**********************************************/


/*************************************************
 * Function : initiateEmergStop
 * Desc     : Check for a block on Pad1
 * Return   : void
 * Params   : void
 *^z**********************************************/
static void initiateEmergStop()
{
   OSTaskCreate(appTaskEmergStop,
                (void *)0,
                (OS_STK *)&appTaskEmergStopStk[TASK_STK_SIZE - 1],
                APP_TASK_EMERG_STOP_PRIO);
}
/*z^**********************************************/


/*************************************************
 * Function : checkPadSensorOne
 * Desc     : Check for a block on Pad1
 * Return   : 
 * Params   :
 *^z**********************************************/
static void checkPadSensorOne
(
   messageData_t  *msg
)
{
   if (controlItemPresent(CONTROL_SENSOR_1))
   {
      msg->targetID      = ID_ARM1;
      msg->originatorID  = MY_ID;
      msg->content       = MSG_SENS1_FULL;
   }
   else
   {
      msg->targetID      = ID_ARM1;
      msg->originatorID  = MY_ID;
      msg->content       = MSG_SENS1_EMPTY;
   }

}
/*z^**********************************************/


/*************************************************
 * Function : checkPadSensorTwo
 * Desc     : Check for a block on Pad2
 * Return   : 
 * Params   :
 *^z**********************************************/
static void checkPadSensorTwo
(
   messageData_t  *msg
)
{
   if (controlItemPresent(CONTROL_SENSOR_2))
   {
      msg->targetID      = ID_ARM2;
      msg->originatorID  = MY_ID;
      msg->content       = MSG_SENS2_FULL;
   }
   else
   {
      msg->targetID      = ID_ARM2;
      msg->originatorID  = MY_ID;
      msg->content       = MSG_SENS2_EMPTY;
   }
}
/*z^**********************************************/
/*z^*****************************************************************************************/
/******************************************* EOF ********************************************/
